﻿using System;
using System.Collections.Generic;

namespace PredatorPrey.Library.Algorithms
{
	// Id		: $Id: AStarSearch.cs 39 2009-04-23 21:38:07Z kristof@kristofmattei.be $
	// Date		: $Date: 2009-04-23 21:38:07 +0000 (Thu, 23 Apr 2009) $
	// Author	: $Author: kristof@kristofmattei.be $
	// Revision	: $Revision: 39 $
	/// <summary>
	/// Uses the A* algoritm to search a path in the Field
	/// </summary>
	/// </summary>
	public class AStarSearch : Search
	{
		/// <summary>
		/// Initialize an AStarSearch
		/// </summary>
		/// <param name="field"></param>
		public AStarSearch(Field field)
			: base(field)
		{

		}

		/// <summary>
		/// Search an optimal path
		/// </summary>
		/// <returns>
		/// An array of Cells, the most optimal solution
		/// </returns>
		public override List<Cell> SearchPath(Cell start, Cell goal)
		{
			//the sets
			List<Cell> closedSet = new List<Cell>();
			List<Cell> openSet = new List<Cell>();

			//The score from the start to the current node (the Cell)
			Dictionary<Cell, double> gScores = new Dictionary<Cell, double>();

			//the score from the current node (The Cell) to the end (the Prey)
			Dictionary<Cell, double> hScores = new Dictionary<Cell, double>();

			//The gScore and hScore of a Cell added together
			Dictionary<Cell, double> fScores = new Dictionary<Cell, double>();

			//The Came From dictionary :)
			Dictionary<Cell, Cell> cameFrom = new Dictionary<Cell, Cell>();

			//so we've got the start and end
			//now we start searching
			//Go sherlock holmes!
			gScores.Add(start, 0);
			hScores.Add(start, this.HeuristicSearch(start, goal));
			fScores.Add(start, (gScores[start] + hScores[start]));

			openSet.Add(start);

			//loop as long as the openSet is not empty
			while (openSet.Count != 0)
			{
				//we look for the node within the openSet with the lowest f score.
				Cell cellWithLowestFScore = this.FindLowest(openSet, fScores);

				//is it the goal?
				if (cellWithLowestFScore == goal)
				{
					return this.ReconstructPath(cameFrom, goal);
				}

				//we checked it, we don't want to check it again
				openSet.Remove(cellWithLowestFScore);
				closedSet.Add(cellWithLowestFScore);

				//loop through all neighbour nodes
				foreach (Cell neighbour in this.Field.GetNeighbours(cellWithLowestFScore))
				{
					//check if we already checked it
					if (closedSet.Contains(neighbour))
						continue;

					//calculate the gScore
					double tentativeGScore = gScores[cellWithLowestFScore] + PathCost.GetPathCost(neighbour.FieldType);

					//is this one better than the rest?
					bool tentativeIsBetter = false;

					//did we check it? if not: add it to the to-be-checked list
					if (!openSet.Contains(neighbour))
					{
						openSet.Add(neighbour);
						hScores.Add(neighbour, HeuristicSearch(neighbour, goal));
						tentativeIsBetter = true;
					}
					//is the current gScore higher than the parent's?
					else if (tentativeGScore < gScores[neighbour])
					{
						tentativeIsBetter = true;
					}

					//if it is better
					if (tentativeIsBetter)
					{
						cameFrom.Add(neighbour, cellWithLowestFScore);
						gScores[neighbour] = tentativeGScore;
						fScores[neighbour] = gScores[neighbour] + hScores[neighbour];
					}

				}
			}

			throw new Exception("Horrible problems");
		}

		/// <summary>
		/// Returns the distance between two points. 
		/// We need to make this generic w/ a delegate or something
		/// </summary>
		/// <param name="cell1">The first Cell</param>
		/// <param name="cell2">The second Cell</param>
		/// <returns>the distance between the cells</returns>
		public double HeuristicSearch(Cell cell1, Cell cell2)
		{
			return Math.Sqrt(Math.Pow((cell2.X - cell1.X), 2) + Math.Pow((cell2.Y - cell1.Y), 2));
		}

		/// <summary>
		/// Finds the set with the lowest value in fScores
		/// </summary>
		/// <param name="set">A set of cells</param>
		/// <param name="fScores">A dictionary of cells and their fScores</param>
		/// <returns></returns>
		private Cell FindLowest(List<Cell> set, Dictionary<Cell, double> fScores)
		{
			Cell lowestCell = new Cell();
			double lowest = double.MaxValue;

			//loop through all cells in the set
			foreach (Cell cell in set)
			{
				//we extract the value in a seperate value
				//because lookup operations are expensive
				double value = fScores[cell];

				//and out of the set we select the lowest cell with the lowest fScore
				if (value < lowest)
				{
					lowestCell = cell;
					lowest = fScores[cell];
				}
			}

			return lowestCell;
		}
	}
}
